{
  "cells": [
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "intro_md"
      },
      "source": [
        "# PlanarProjectionFactor"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "desc_md"
      },
      "source": [
        "The `PlanarProjectionFactor` variants provide camera projection factors specifically designed for scenarios where **the robot or camera moves primarily on a 2D plane** (e.g., ground robots with cameras).\n",
        "They relate a 3D landmark point to a 2D pixel measurement observed by a camera, considering:\n",
        "*   The robot's 2D pose (`Pose2` `wTb`: body in world frame) in the ground plane.\n",
        "*   The camera's fixed 3D pose relative to the robot's body frame (`Pose3` `bTc`: body-to-camera).\n",
        "*   The camera's intrinsic calibration (including distortion, typically `Cal3DS2` or similar).\n",
        "*   The 3D landmark position in the world frame.\n",
        "\n",
        "The core projection logic involves converting the `Pose2` `wTb` to a `Pose3` assuming z=0 and yaw=theta, composing it with `bTc` to get the world-to-camera pose `wTc`, and then using a standard `PinholeCamera` model to project the landmark.\n",
        "\n",
        "Variants:\n",
        "*   `PlanarProjectionFactor1`: Unknown is robot `Pose2` (`wTb`). Landmark, `bTc`, and calibration are fixed. Useful for localization.\n",
        "*   `PlanarProjectionFactor2`: Unknowns are robot `Pose2` (`wTb`) and `Point3` landmark. `bTc` and calibration are fixed. Useful for planar SLAM.\n",
        "*   `PlanarProjectionFactor3`: Unknowns are robot `Pose2` (`wTb`), camera offset `Pose3` (`bTc`), and `Cal3DS2` calibration. Landmark is fixed. Useful for calibration."
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "colab_badge_md"
      },
      "source": [
        "<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/slam/doc/PlanarProjectionFactor.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 1,
      "metadata": {
        "id": "pip_code",
        "tags": [
          "remove-cell"
        ]
      },
      "outputs": [],
      "source": [
        "try:\n",
        "    import google.colab\n",
        "    %pip install --quiet gtsam-develop\n",
        "except ImportError:\n",
        "    pass  # Not running on Colab, do nothing"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 2,
      "metadata": {
        "id": "imports_code"
      },
      "outputs": [],
      "source": [
        "import gtsam\n",
        "import numpy as np\n",
        "from gtsam import (Pose2, Pose3, Point3, Point2, Rot3, Cal3DS2, Values,\n",
        "                   PlanarProjectionFactor1, PlanarProjectionFactor2, PlanarProjectionFactor3)\n",
        "from gtsam.symbol_shorthand import X, L, C, O"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "factor1_header_md"
      },
      "source": [
        "## 1. `PlanarProjectionFactor1` (Localization)"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "factor1_desc_md"
      },
      "source": [
        "Used when the landmark, camera offset (`bTc`), and calibration (`calib`) are known, and we want to estimate the robot's `Pose2` (`wTb`)."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 3,
      "metadata": {
        "id": "factor1_example_code"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Ground Truth Pose2: (1, 0, 0.785398)\n",
            "\n",
            "Calculated Measurement: [ 909.25565099 1841.1002863 ]\n",
            "Factor 1:   keys = { x0 }\n",
            "isotropic dim=2 sigma=1.5\n",
            "\n",
            "Error at ground truth: 0.0\n",
            "Error at noisy pose: 3317.6472637491106\n"
          ]
        }
      ],
      "source": [
        "# Known parameters\n",
        "landmark_pt = Point3(2.0, 0.5, 0.5)\n",
        "body_T_cam = Pose3(Rot3.Yaw(-np.pi / 2), Point3(0.1, 0, 0.2))  # Cam fwd = body +y\n",
        "calib = Cal3DS2(fx=500, fy=500, s=0, u0=320, v0=240, k1=0, k2=0, p1=0, p2=0)\n",
        "measurement_noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.5)  # Pixels\n",
        "\n",
        "# Assume ground truth pose and calculate expected measurement\n",
        "gt_pose2 = Pose2(1.0, 0.0, np.pi / 4)\n",
        "gt_world_T_cam = Pose3(gt_pose2) * body_T_cam\n",
        "gt_camera = gtsam.PinholeCameraCal3DS2(gt_world_T_cam, calib)\n",
        "measured_pt2 = gt_camera.project(landmark_pt)\n",
        "print(f\"Ground Truth Pose2: {gt_pose2}\")\n",
        "print(f\"Calculated Measurement: {measured_pt2}\")\n",
        "\n",
        "# Create the factor\n",
        "factor1 = PlanarProjectionFactor1(\n",
        "    X(0), landmark_pt, measured_pt2, body_T_cam, calib, measurement_noise\n",
        ")\n",
        "factor1.print(\"Factor 1: \")\n",
        "\n",
        "# Evaluate error\n",
        "values = Values()\n",
        "values.insert(X(0), gt_pose2)  # Error should be zero here\n",
        "error1_gt = factor1.error(values)\n",
        "print(f\"\\nError at ground truth: {error1_gt}\")\n",
        "\n",
        "noisy_pose2 = Pose2(1.05, 0.02, np.pi / 4 + 0.05)\n",
        "values.update(X(0), noisy_pose2)\n",
        "error1_noisy = factor1.error(values)\n",
        "print(f\"Error at noisy pose: {error1_noisy}\")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "factor2_header_md"
      },
      "source": [
        "## 2. `PlanarProjectionFactor2` (Planar SLAM)"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "factor2_desc_md"
      },
      "source": [
        "Used when the camera offset (`bTc`) and calibration (`calib`) are known, but both the robot `Pose2` (`wTb`) and the `Point3` landmark position are unknown."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 4,
      "metadata": {
        "id": "factor2_example_code"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Factor 2:   keys = { x0 l0 }\n",
            "isotropic dim=2 sigma=1.5\n",
            "\n",
            "Error at ground truth: 0.0\n",
            "Error with noisy landmark: 8066.192649473802\n"
          ]
        }
      ],
      "source": [
        "factor2 = PlanarProjectionFactor2(\n",
        "    X(0), L(0), measured_pt2, body_T_cam, calib, measurement_noise\n",
        ")\n",
        "factor2.print(\"Factor 2: \")\n",
        "\n",
        "# Evaluate error\n",
        "values = Values()\n",
        "values.insert(X(0), gt_pose2)\n",
        "values.insert(L(0), landmark_pt)  # Error should be zero\n",
        "error2_gt = factor2.error(values)\n",
        "print(f\"\\nError at ground truth: {error2_gt}\")\n",
        "\n",
        "noisy_landmark = Point3(2.1, 0.45, 0.55)\n",
        "values.update(L(0), noisy_landmark)\n",
        "error2_noisy = factor2.error(values)\n",
        "print(f\"Error with noisy landmark: {error2_noisy}\")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "factor3_header_md"
      },
      "source": [
        "## 3. `PlanarProjectionFactor3` (Calibration)"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "factor3_desc_md"
      },
      "source": [
        "Used when the landmark position is known, but the robot `Pose2` (`wTb`), the camera offset `Pose3` (`bTc`), and the `Cal3DS2` calibration are unknown."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 5,
      "metadata": {
        "id": "factor3_example_code"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Factor 3:   keys = { x0 o0 c0 }\n",
            "isotropic dim=2 sigma=1.5\n",
            "\n",
            "Error at ground truth: 0.0\n",
            "Error with noisy calibration: 92.30212176019934\n"
          ]
        }
      ],
      "source": [
        "offset_key = O(0)\n",
        "calib_key = C(0)\n",
        "\n",
        "factor3 = PlanarProjectionFactor3(X(0), offset_key, calib_key, landmark_pt, measured_pt2, measurement_noise)\n",
        "factor3.print(\"Factor 3: \")\n",
        "\n",
        "# Evaluate error\n",
        "values = Values()\n",
        "values.insert(X(0), gt_pose2)\n",
        "values.insert(offset_key, body_T_cam)\n",
        "values.insert(calib_key, calib) # Error should be zero\n",
        "error3_gt = factor3.error(values)\n",
        "print(f\"\\nError at ground truth: {error3_gt}\")\n",
        "\n",
        "noisy_calib = Cal3DS2(fx=510, fy=495, s=0, u0=322, v0=241, k1=0, k2=0, p1=0, p2=0)\n",
        "values.update(calib_key, noisy_calib)\n",
        "error3_noisy = factor3.error(values)\n",
        "print(f\"Error with noisy calibration: {error3_noisy}\")"
      ]
    }
  ],
  "metadata": {
    "kernelspec": {
      "display_name": "py312",
      "language": "python",
      "name": "python3"
    },
    "language_info": {
      "codemirror_mode": {
        "name": "ipython",
        "version": 3
      },
      "file_extension": ".py",
      "mimetype": "text/x-python",
      "name": "python",
      "nbconvert_exporter": "python",
      "pygments_lexer": "ipython3",
      "version": "3.12.6"
    }
  },
  "nbformat": 4,
  "nbformat_minor": 0
}
